﻿#include "pickmovez.h"
#include <QDebug>
#include <iostream>
#include <QtConcurrent/QtConcurrent>
#include "robotCtrlPanl.h"
#include "robotEvent.h"
#include "robotMC.h"
#include "robotMecUnit.h"
#include "robotMotion.h"
#include "robotSystem.h"
#include "robotFile.h"
#include "robotIO.h"
#include "robotPGM.h"
#include <thread>

string currtool="";
string currobj="";
float jogacc=0;
float jogdacc=0;
float prospeed=0;
HelperJogMode jogmode=HELPER_JOG_MODE_UNKNOWN;
HelperRobJointTarget* curjoint=nullptr;
HelperRobTarget* curpos=nullptr;
bool isconnected=false;
void *robhandle=nullptr;
string MecName;
string robotip;
QString ErrorDes;
int logindex;

QMap<QString,int> rob_DO = {
    {"DO20_10",0},{"DO20_11",0},{"DO20_12",0},{"DO20_13",0},{"DO20_14",0},{"DO20_15",0},{"DO20_16",0},{"DO20_17",0},{"DO20_18",0},{"DO20_19",0},{"DO20_20",0},{"DO20_21",0},
    {"DO20_22",0},{"DO20_23",0}
};
QMap<QString,int> rob_DI = {
    {"DI20_10",0},{"DI20_11",0},{"DI20_12",0},{"DI20_13",0},{"DI20_14",0},{"DI20_15",0},{"DI20_16",0},{"DI20_17",0},{"DI20_18",0},{"DI20_19",0},{"DI20_20",0},{"DI20_21",0},
    {"DI20_22",0},{"DI20_23",0}
};


using namespace Taike;

PickMoveZ* PickMoveZ::_self = nullptr;

void fisconnected(void* handle,DasDataType type,const char* name,int value,int sim)
{
    qDebug()<< "connected" <<QString::fromLatin1(name)<<value<<sim;
}
void eventChanged(void *handle, unsigned long seqno)
{
    qDebug()<<seqno;
    MsgPackage* msgs = new MsgPackage;
    unsigned long index=0;
    GetLastEventLogs(handle,1,&msgs,&index);
    cout<<"event changed ******************"<<string(msgs->msgName)<<string(msgs->msgTitle)<<endl;

}
void findip(const char* ip)
{
    cout<<"find ip " << ip <<endl;
}
void lostip(const char* ip)
{
    cout<<"lost ip " << ip <<endl;
}
string tostring(ServoState statu)
{
    string str;
    switch (statu) {
    case 0:
        str="SERVO_UNKNOWN";
        break;
    case 1:
        str="SERVO_ERROR";
        break;
    case 2:
        str="SERVO_INITEDS";
        break;
    case 3:
        str="SERVO_ENABLED";
        break;
    case 4:
        str="SERVO_DISABLED";
        break;
    default:
        break;
    }
    return str;
}
QString char2qstring(char* p,int len)
{
    QString str;
    for(int i=0;i<len-1;++i)
    {
        if(*(p+i) == '\0')
            str.append(' ');
        else
            str.append(*(p+i));
    }
    return str.trimmed();
}

void crossProduct(const QVector3D v1,const QVector3D v2,QVector3D& result)
{
    result.setX(v1.y()*v2.z()-v1.z()*v2.y());
    result.setY(v1.z()*v2.x()-v1.x()*v2.z());
    result.setZ(v1.x()*v2.y()-v1.y()*v2.x());
}
int pointsColliner(const QVector3D A,const QVector3D B,const QVector3D C)
{
    qDebug()<<A<<B<<C;
    QVector3D AB,BC,crossProductResult;
    AB.setX(B.x() - A.x());
    AB.setY(B.y() - A.y());
    AB.setZ(B.z() - A.z());

    BC.setX(C.x() - B.x());
    BC.setY(C.y() - B.y());
    BC.setZ(C.z() - B.z());
    qDebug()<<"AB BC :"<<AB<<BC;
    // 计算叉乘
    crossProduct(AB, BC, crossProductResult);
    qDebug()<<"cross reslut "<<crossProductResult;
    // 如果叉乘结果的模长为0，则三点共线

    if(qAbs(crossProductResult.x())<1e-4 && qAbs(crossProductResult.y())< 1e-4 && qAbs(crossProductResult.z()) < 1e-4)
        return 0;
    return 1;
}

PickMoveZ::PickMoveZ()
{

    timer=new QTimer();
    QObject::connect(timer,SIGNAL(timeout()),this,SLOT(updatepos()));
    timer->setSingleShot(true);
    curjoint = new HelperRobJointTarget();
    curpos=new HelperRobTarget();
    jogtimer = new QTimer();
    QObject::connect(jogtimer,SIGNAL(timeout()),this,SLOT(jogslot()));
    movebeat=new QTimer();
    QObject::connect(movebeat,SIGNAL(timeout()),this,SLOT(movemonitor()));
    moni=new monitor();
    QObject::connect(moni,SIGNAL(sig_ErrorOcc()),this,SLOT(errorocced()));
    isconnected=false;
    logindex=-1;
    qRegisterMetaType<QVector<double> >("QVector<double>");
    speeddata = new SpeedData{200.000,200.000,200.000,70.000};
    ExecutionMode=HelperExecutionMode::EXECUTION_MODE_CONTINOUS;
}

PickMoveZ::~PickMoveZ()
{
    delete moni;
}

void PickMoveZ::setip(QString ip)
{
    robotip=ip.toStdString();
}

bool PickMoveZ::netstatus()
{
    return isconnected;
}

void PickMoveZ::connect()
{
    isinit=false;
    if(robotip.empty())
        return;
    std::thread td([]{
        robhandle = ConnectRobotController(robotip.c_str(),NULL,NULL);
    });
    td.detach();
    trynumber=0;
    //    QFuture<void> f1 = QtConcurrent::run([=](){
    //        handle = ConnectRobotController(robotip.c_str(),NULL,NULL);
    //    });
    //    f1.waitForFinished();
    evt=new QTimer();
    QObject::connect(evt,&QTimer::timeout,[=](){
        if(robhandle == nullptr)
        {
            std::cout<<"FAIL ConnectRobotController ip("+robotip+")"<<endl;
            if(timer->isActive())
                timer->stop();
            trynumber++;
            if(trynumber>10)
            {
                evt->stop();
            }
            emit networkconnected(false);
        }else
        {
            isconnected=true;
            cout<<string("Success ConnectRobotController ip("+robotip+")")<<endl;
            timer->start(1000);
            emit networkconnected(true);
            evt->stop();
        }
    });
    evt->start(1000);
}

void PickMoveZ::disconnect()
{
    if(timer->isActive())
        timer->stop();
    isconnected=false;
    if(evt->isActive())
        evt->stop();
    //    if(handle)
    //        DisconnectRobotController(handle);

}

void PickMoveZ::setenable(bool ok)
{
    if(ok)
    {
        cout<<"robot enable...."<<endl;
        SetCtrlEnable(robhandle);
    }
    else
    {
        cout<<"robot disable..."<<endl;
        SetCtrlDisable(robhandle);
    }
}

void PickMoveZ::setEMode()
{
    SetESMode(robhandle);
}

void PickMoveZ::setEReset()
{
    SetESMode(robhandle);
}

void PickMoveZ::SetCoordSysterm(int c)
{
    CoordSystem sys;
    switch (c) {
    case 0:
        sys=CoordSystem::COORDSYSTEM_WORLD;
        break;
    case 1:
        sys=CoordSystem::COORDSYSTEM_BASE;
        break;
    case 2:
        sys=CoordSystem::COORDSYSTEM_TOOL;
        break;
    case 3:
        sys=CoordSystem::COORDSYSTEM_WOBJ;
        break;
    default:
        sys=COORDSYSTEM_UNDEFINED;
        break;
    }
    SetCoordSystem(robhandle,MecName.c_str(),sys);
}

void PickMoveZ::setjogmodel(int md)
{
    HelperJogMode model;
    switch (md) {
    case 0:
        model=HELPER_JOG_MODE_CARTESIAN;
        break;
    case 1:
        model=HELPER_JOG_MODE_AXISGROUP1;
        break;
    case 2:
        model=HELPER_JOG_MODE_AXISGROUP2;
        break;
    case 3:
        model=HELPER_JOG_MODE_GOTOPOSITION;
        break;
    case 4:
        model=HELPER_JOG_MODE_GOTOJOINT;
        break;
    case 5:
        model=HELPER_JOG_MODE_DRAGTEACH;
        break;
    default:
        model=HELPER_JOG_MODE_UNKNOWN;
        break;
    }
    SetRobJogMode(robhandle,MecName.c_str(),model);
}

void PickMoveZ::setprospeed(float sp)
{
    SetPgmRate(robhandle,sp);
}

void PickMoveZ::setJogAcc(float acc)
{
    SetJogAcc(robhandle,acc);
}

void PickMoveZ::setJogDAcc(float dacc)
{
    SetJogDeAcc(robhandle,dacc);
}

void PickMoveZ::jogJoint(int JNum, int dir, int step)
{
    HelperJogMode md;
    GetRobJogMode(robhandle,MecName.c_str(),&md);
    if(md!= HELPER_JOG_MODE_AXISGROUP1)
    {
        setjogmodel(1);
    }
    jogjointnum=JNum;
    jogdir=dir;
    jogstep = step;
    jogtimer->start(150);

}

void PickMoveZ::jogPos(int Index, int dir, int step)
{
    HelperJogMode md;
    GetRobJogMode(robhandle,MecName.c_str(),&md);
    if(md!= HELPER_JOG_MODE_CARTESIAN)
    {
        setjogmodel(0);
    }
    jogjointnum=Index;
    jogdir=dir;
    jogstep = step;
    jogtimer->start(150);

}

void PickMoveZ::stopJog()
{
    if(jogtimer->isActive())
        jogtimer->stop();
    RobJogVector vector;
    for(int i=0;i<_dof;++i)
    {
        vector.element[i]=0;
    }
    IncrementalMode increment=IncrementalMode::INCREMENTAL_MODE_USER;
    SetJogCommand(robhandle,vector,increment);
}

void PickMoveZ::movepp(QVector<double> target)
{
    qDebug()<<"move p2p"<<target;
    if(target.size()<_dof)
        return;
    HelperRobJointTarget *pt=new HelperRobJointTarget;
    for(int i=0;i<target.size();++i)
    {
        pt->rax[i]= target.at(i);
    }
    for(int j=0;j<MAX_NUM_OF_EXT_JOINTS;++j)
    {
        pt->eax[j]= 0;
    }
    //SpeedData* data = new SpeedData{200.000,200.000,200.000,70.000};
    ZoneData* z5=new ZoneData();
    MC_Init(robhandle);
    MC_MoveAbsJ(robhandle,pt,speeddata,0,0);
    MC_Start(robhandle);
    isarrived(1);

}

void PickMoveZ::moveL(QVector<double> target)
{
    qDebug()<<"moveL"<<target;
    if(target.size()<6)
        return;
    HelperRobTarget pt = *curpos;
    if(qAbs(target.at(0)-pt.pos.x)<1e-4 && qAbs(target.at(1)-pt.pos.y)<1e-4 && qAbs(target.at(2)-pt.pos.z)<1e-4)
    {
        cout<<"target point is same with current"<<endl;
        return;
    }

    pt.pos.x=target.at(0);
    pt.pos.y=target.at(1);
    pt.pos.z=target.at(2);

    double a,b,c;
    a=target.at(3)*M_PI/180;
    b=target.at(4)*M_PI/180;
    c=target.at(5)*M_PI/180;
    pt.orient=ToQuaternion(c,b,a);

    //SpeedData* data = new SpeedData{200.000,200.000,200.000,70.000};
    ZoneData* z5=new ZoneData();
    MC_Init(robhandle);
    MC_MoveL(robhandle,&pt,speeddata,0,0);
    MC_Start(robhandle);
    isarrived(1);
    cout<<"moveL finished"<<endl;
}



void PickMoveZ::moveC(QVector<double> pass, QVector<double> end)
{
    qDebug()<<"MoveC "<< pass<<end;
    if(pass.size()!=6|| end.size()!=6)
    {
        cout<<"error paramet when execute moveC";
        return;
    }

    HelperRobTarget ptpass = *curpos;
    HelperRobTarget ptend = ptpass;
    QVector3D A(ptpass.pos.x,ptpass.pos.y,ptpass.pos.z);
    QVector3D B(pass.at(0),pass.at(1),pass.at(2));
    QVector3D C(end.at(0),end.at(1),end.at(2));
    int res=pointsColliner(A,B,C);

    qDebug()<<"A=="<<A;
    qDebug()<<"B=="<<B;
    qDebug()<<"C=="<<C;
    qDebug()<<"res=="<<res;

    if(res==0)
    {
        cout<<"paramets points is collinear!"<<endl;
        return;
    }
    ptpass.pos.x=pass.at(0);
    ptpass.pos.y=pass.at(1);
    ptpass.pos.z=pass.at(2);
    double a,b,c;
    a=pass.at(3)*M_PI/180;
    b=pass.at(4)*M_PI/180;
    c=pass.at(5)*M_PI/180;
    ptpass.orient=ToQuaternion(c,b,a);

    ptend.pos.x=end.at(0);
    ptend.pos.y=end.at(1);
    ptend.pos.z=end.at(2);
    a=end.at(3)*M_PI/180;
    b=end.at(4)*M_PI/180;
    c=end.at(5)*M_PI/180;
    ptend.orient=ToQuaternion(c,b,a);

    //SpeedData* data = new SpeedData{200.000,200.000,200.000,70.000};
    ZoneData* z5=new ZoneData();
    //   LoadData load0 = LoadData{0.00,{{0.000,0.000,0.000},{1.000000,0.000000,0.000000,0.000000}},{{0.000,0.000,0.000,0.000,0.000,0.000}}};
    //    WobjData wobj0 = WobjData{0,1,"",{{0.000,0.000,0.000},{1.000000,0.000000,0.000000,0.000000}},{{0.000,0.000,0.000},{1.000000,0.000000,0.000000,0.000000}}};
    //    ToolData tool0 = ToolData{1,{{0.000,0.000,0.000},{1.000000,0.000000,0.000000,0.000000}},{0.00,{0.000,0.000,0.000},{1.000000,0.000000,0.000000,0.000000},{0.000,0.000,0.000,0.000,0.000,0.000}}};
    MC_Init(robhandle);
    MC_Start(robhandle);
    MC_ConfJ(robhandle, 0);
    MC_ConfL(robhandle, 0);

    MC_SetZone(robhandle, z5);
    //MC_SetLoad(handle, &load0);
    //MC_SetWobj(handle, &wobj0);
    MC_SetSpeed(robhandle, speeddata);
    MC_MoveC(robhandle,&ptpass,&ptend,speeddata,0,0);

    MC_WaitEndOfMotion(robhandle,-1);
    emit sig_robotstatus("info:move finished");
    cout<<"moveC finished"<<endl;
}

void PickMoveZ::movestop()
{
    MC_Stop(robhandle);
    if(movebeat->isActive())
        movebeat->stop();
}

int PickMoveZ::isarrived(int index)
{
    movepointsize = index;
    if(movepointsize==0 && movepointsize>2)
    {
        cout<<"error point size"<<endl;
        return -1;
    }
    movebeat->start(100);
    return 0;
}

monitor *PickMoveZ::getmonitor()
{
    return moni;
}

void PickMoveZ::LoadProgram(QString path)
{
    string fileName=path.toStdString();
    LoadPGM(robhandle,fileName.c_str(),1);

}

void PickMoveZ::setProRunMode(bool isstep)
{
    if(isstep)
        ExecutionMode=HelperExecutionMode::EXECUTION_MODE_STEPIN;
    else
        ExecutionMode=HelperExecutionMode::EXECUTION_MODE_CONTINOUS;

}

void PickMoveZ::getProstatus()
{
    PGMState pgmState;
    TaskState taskState;
    char programName[NET_NAME_MAX + 1];
    CheckPgmState(robhandle, &pgmState, &taskState, programName);
    cout<< pgmState <<"  "<<taskState<<"  "<<programName<<endl;
}

int PickMoveZ::GetDI(QString name)
{
    if(rob_DI.contains(name))
        return rob_DI[name];
    return -1;
}

void PickMoveZ::SetDO(QString name, int i)
{
    if(rob_DO.contains(name))
    {
        IoItem item;
        //GetExtraIOData(robhandle,&item,"DO",name.toStdString().c_str());
        item.value=i;
        SetExtraIOData(robhandle,"DO",name.toStdString().c_str(),&item);
    }
}

void PickMoveZ::getState()
{
    QString str;
    RobotControllerInfo info;
    GetAllMecUnits(robhandle,&info);
    str.append(QString("All Mec Units : %1;\n").arg(info.mecNum));
    cout<<"All Mec Units : "<< info.mecNum <<endl;
    if(info.mecNum > MOTION_MECUNIT_MAX_NUM)
        return;
    for(int i=0;i<info.mecNum;++i)
    {
        string tcptype,robottype;
        switch (info.mecUnits[i].robotType) {
        case 0:
            robottype="IRB_NONE";
            break;
        case 1:
            robottype="GENE_ZJV4";
            break;
        case 2:
            robottype="IRB_NEZHA";
            break;
        case 3:
            robottype="IRB_UR_BASE";
            break;
        case 4:
            robottype="GENE_SCARA";
            break;
        case 5:
            robottype="IRB_ELBOW_BASE";
            break;
        default:
            break;
        }

        switch(info.mecUnits[i].tcpType){
        case 0:
            tcptype="NONE_ROBUNIT";
            break;
        case 1:
            tcptype="TCPROBOT";
            break;
        case 2:
            tcptype="MULTIAXES";
            break;
        case 3:
            tcptype="SINGLEAXIS";
            break;
        default:
            break;
        }
        QString mecinfo;
        mecinfo= QString("id = %1;\n name = %2;\n model = %3;\n mumOfAxes = %4;\n  robotType = %5;\n unitType = %6;\n tcpTpye = %7;\n")
                .arg(QString::number((int)info.mecUnits[i].id)).arg(QString::fromLatin1(info.mecUnits[i].name))
                .arg(((info.mecUnits[i].mode==0)?"ROBMECHUNITMODE_DEACTIVATED":"ROBMECHUNITMODE_ACTIVATED"))
                .arg(QString::number(info.mecUnits[i].numOfAxes)).arg(QString::fromStdString(robottype))
                .arg((info.mecUnits[i].unitType==0) ? "isrobot":"isaxis").arg(QString::fromStdString(tcptype));
        str.append(mecinfo);
        cout<<"id = "<<(int)info.mecUnits[i].id
           <<" name = "<<string(info.mecUnits[i].name)
          <<" model = "<<((info.mecUnits[i].mode==0)?"ROBMECHUNITMODE_DEACTIVATED":"ROBMECHUNITMODE_ACTIVATED")
         <<" mumOfAxes = "<<info.mecUnits[i].numOfAxes
        <<" robotType = "<<robottype
        <<" unitType = "<<((info.mecUnits[i].unitType==0) ? "isrobot":"isaxis")
        <<" tcpTpye = "<<tcptype<<endl;
        if(info.mecUnits[i].unitType == 0)
        {
            MecName= string(info.mecUnits[i].name);
            _dof=info.mecUnits[i].numOfAxes;
        }
        else
            return;
    }
    float dh[8],trans[7];
    GetRobDHPara(robhandle,MecName.c_str(),dh,trans);
    for(int j=0;j<_dof;++j)
    {
        str.append(QString::number(dh[j])).append(" ");
        cout<<dh[j]<<" ";
    }
    str.append("\n");
    cout<<endl;
    for(int j=0;j<_dof;++j)
    {
        str.append(QString::number(trans[j])).append(" ");
        cout << trans[j]<<" ";
    }
    str.append("\n");
    cout <<endl;
    float a[7],alpha[7],d[7],theta[7];
    GetRobURDHPara(robhandle,MecName.c_str(),a,alpha,theta,d);
    for(int i=0;i<_dof;++i)
    {
        str.append(QString::number(i));
        str.append(QString(" a  alpha d theta : %1 %2 %3 %4;\n").arg(QString::number(a[i])).arg(QString::number(alpha[i])).arg(QString::number(d[i])).arg(QString::number(theta[i])));
        cout <<i <<" a  alpha d theta : "<<a[i]<<" "<<alpha[i] <<" "<<d[i]<<" "<<theta[i]<<endl;
    }
    MotorSpeed* speedlimit= new MotorSpeed[7];
    int axis;
    GetMotorSpeedLimit(robhandle,MecName.c_str(),&speedlimit,&axis);
    for(int i=0;i<_dof;++i)
    {
        QString substr=QString("MotorSpeedLimit %1 speed %2 speedLimit %3 \n").arg(QString(speedlimit[i].name))
                .arg(QString::number(speedlimit[i].motorSpeed)).arg(QString::number(speedlimit->motorSpeedLimit));
        str.append(substr);
        cout<<"GetMotorSpeedLimit "<<string(speedlimit[i].name)<<" speed "<<speedlimit[i].motorSpeed<<" speedLimit "<<speedlimit->motorSpeedLimit<<endl;
    }
    JointsLimit* limits= new JointsLimit[7];
    GetMechUnitLimit(robhandle,MecName.c_str(),&limits,&axis);
    for(int i=0;i<_dof;++i)
    {
        QString substr= "Joint Limits : "+QString(limits[i].name)+" "+QString::number(limits[i].leftRange)+" "+QString::number(limits[i].rightRange)+" "+((limits[i].type==0)?"TRANS_JOINT":"ROTAT_JOINT")+"\n";
        str.append(substr);
        cout<<"Joint Limits : "<< string(limits[i].name)<<" "<<limits[i].leftRange<<" "<<limits[i].rightRange<<" "<<((limits[i].type==0)?"TRANS_JOINT":"ROTAT_JOINT")<<endl;
    }
    for (int i = 0; i < 6; i++)
    {
        char vid[100];
        char pid[100];
        char fwver[100];
        char hwver[100];
        GetServoVendorID(robhandle, MecName.c_str(), i,vid, pid, fwver, hwver);
        string s1 = string(vid);
        string s2 = string(pid);
        string s3 = string(fwver);
        //string s4 = string(hwver); //QString::fromLatin1(hwver).toStdString();//char2qstring(hwver,100).toStdString();
        //std::copy(std::begin(hwver),std::end(hwver)-1,s4);
        //cout<<s4.size()<<endl;
        QString substr="ServoVendorID "+ QString::number(i) + " vid pid fwver hwver:" +QString::fromStdString(s1) + "," + QString::fromStdString(s2) + "," + QString::fromStdString(s3) +"\n";
        cout<<"GetServoVendorID "<< i << " vid pid fwver hwver:" + s1 + "," + s2 + "," + s3 <<endl;
        str.append(substr);
    }
    int* num= new int[10];
    int axisNo = 0;
    GetServoHomeEncoder(robhandle,MecName.c_str(),&axisNo,&num);
    //cout<<"GetServoHomeEncoder "<< axisNo<< " : "<<endl;
    for(int i=0;i<axisNo;++i)
    {
        QString substr="Servo home encoder data "+QString::number(i)+ " : "+QString::number(num[i])+ "\n";
        str.append(substr);
        cout<<"Servo home encoder data"<<i<<" : "<< num[i];
    }
    cout<<endl;

    Pose pose;
    GetBaseFrame(robhandle,MecName.c_str(),&pose);
    QString baseframe = "current Baseframe : ["+ QString::number(pose.pos.x) +" "+QString::number(pose.pos.y) +" "+QString::number(pose.pos.z)+"] ["
            +QString::number(pose.orient.u0)+" "+QString::number(pose.orient.u1)+" "+QString::number(pose.orient.u2)+" "+QString::number(pose.orient.u3)+"]\n";
    cout<<"current Baseframe : ["<<pose.pos.x<<" "<<pose.pos.y<<" "<<pose.pos.z<<"] [";
    cout<<pose.orient.u0<<" "<<pose.orient.u1<<" "<<pose.orient.u2<<" "<<pose.orient.u3<<"]"<<endl;
    str.append(baseframe);
    HelperJogMode model;
    GetRobJogMode(robhandle,MecName.c_str(),&model);
    cout<<"current jog mode is "<<model<<endl;
    SetActiveMechUnit(robhandle,MecName.c_str());
    emit sig_robotstatus(str);
    //    char sysname[200];
    //    char sysid[200];
    //    GetSystemName(handle,sysname);
    //    cout<<"Robot System "<<string(sysname);
    //    GetSystemID(handle,sysid);
    //    cout<<" id : "<<sysid<<endl;
    //    char Ctrlname[200];
    //    GetCtrlName(handle,Ctrlname);
    //    cout<<" Ctrl name : "<< Ctrlname;
    //    char ctrlid[200];
    //    GetCtrlID(handle,ctrlid);
    //    cout<< " id : "<<ctrlid<<endl;

}

void PickMoveZ::checkip(const char *ip)
{
    qDebug()<<"find ip";
}

void PickMoveZ::updatepos()
{
    if(!isinit)
    {
        getState();
        moni->start();
    }
    isinit=true;
}

void PickMoveZ::jogslot()
{
    RobJogVector vector;
    for(int i=0;i<_dof;++i)
    {
        if(i==jogjointnum)
        {
            vector.element[jogjointnum]= jogdir*1024;
        }
        else
            vector.element[i]=0;
    }
    //cout<<vector.element[0]<<" "<<vector.element[1]<<" "<<vector.element[2]<<" "<<vector.element[3]<<" "<<vector.element[4]<<" "<<vector.element[5]<<endl;
    IncrementalMode increment=INCREMENTAL_MODE_NONE;
    switch (jogstep) {
    case 0:
        increment=IncrementalMode::INCREMENTAL_MODE_NONE;
        break;
    case 1:
        increment=IncrementalMode::INCREMENTAL_MODE_SMALL;
        break;
    case 2:
        increment=IncrementalMode::INCREMENTAL_MODE_MEDIUM;
        break;
    case 3:
        increment=IncrementalMode::INCREMENTAL_MODE_LARGE;
        break;
    default:
        increment=IncrementalMode::INCREMENTAL_MODE_USER;
        break;
    }
    SetJogCommand(robhandle,vector,increment);
}

void PickMoveZ::movemonitor()
{
    int size=0;
    MC_GetReachedPoint(robhandle,0,&size);
    if(movepointsize==size)
    {
        emit sig_robotstatus("info : move finished!");
        if(movebeat->isActive())
            movebeat->stop();
    }

}

void PickMoveZ::errorocced()
{
    QString errorstr="Error: ";
    errorstr.append(ErrorDes);
    qDebug()<<errorstr;
    emit sig_robotstatus(errorstr);
}

void PickMoveZ::clearerror()
{
    ErrorDes.clear();
    logindex=0;
    qDebug()<<"clear error";
    ClearAllError(robhandle,MecName.c_str());
}

void PickMoveZ::setspeed(int sp)
{
    speeddata->extMS= sp*10.0f;
    speeddata->oriRadS= sp*10.0f;
    speeddata->tcpMS= sp*10.0f;
}

int PickMoveZ::Getspeed()
{
    return speeddata->tcpMS/10;
}
void monitor::run()
{
    while(isconnected)
    {
        ServoState statu;
        GetServoStatus(robhandle,MecName.c_str(),&statu);
        if(statu != ServoState::SERVO_ENABLED)
        {
            cout<<"GetServoStatus : "<<tostring(statu)<<endl;
        }
        float acc,dacc;
        GetJogDeAcc(robhandle,&acc);
        if(acc != jogacc)
        {
            jogacc=acc;
        }
        GetJogDeAcc(robhandle,&dacc);
        if(dacc != jogdacc)
        {
            jogdacc=dacc;
        }
        char tool[100];
        GetRobToolName(robhandle,MecName.c_str(),tool);
        currtool=string(tool);
        char wold[100];
        GetRobWobjName(robhandle,MecName.c_str(),wold);
        currobj=string(wold);
        float rate;
        GetPgmRate(robhandle,&rate);
        if(prospeed != rate)
        {
            prospeed=rate;
        }
        MsgPackage* msgs = new MsgPackage;
        unsigned long index=0;
        GetLastEventLogs(robhandle,1,&msgs,&index);
        if(msgs->msgErrCode != logindex && QString(msgs->msgType) == "Error")
        {
            logindex = msgs->msgErrCode;
            ErrorDes.append("msgs->msgType :").append(msgs->msgType).append("\n")
                    .append("msgs->msgDate :").append(msgs->msgDate).append("\n")
                    .append("msgs->msgDec :").append(QString(msgs->msgDec)).append("\n")
                    .append("msgs->msgName :").append(QString(msgs->msgName)).append("\n")
                    .append("msgs->msgTitle :").append(QString(msgs->msgTitle)).append("\n")
                    .append("msgErrCode :").append(QString::number(msgs->msgErrCode));
            emit sig_ErrorOcc();
            //            qDebug()<<"event changed ******************\n"
            //                <<"msgs->msgType "<<msgs->msgType<<"\n"
            //                <<"msgs->msgDate " <<msgs->msgDate<<"\n"
            //                <<"msgs->msgDec " <<QString(msgs->msgDec)<<"\n"
            //                <<"msgs->msgName " <<QString(msgs->msgName)<<"\n"
            //                <<"msgs->msgTitle " <<QString(msgs->msgTitle)<<"\n"
            //                <<"msgs->sequenceNumber " <<msgs->sequenceNumber<<"\n"
            //                <<"msgs->msgDomain " <<msgs->msgDomain<<"\n"
            //                <<"msgs->msgErrCode " <<msgs->msgErrCode<<"\n";
        }
        //

        HelperJogMode mode;
        GetRobJogMode(robhandle,MecName.c_str(),&mode);
        if(jogmode != mode)
        {
            jogmode=mode;
            cout<<"current jog mode is "<<mode<<endl;

        }
        TeachJointTarget(robhandle,MecName.c_str(),curjoint);
        CoordSystem coord=CoordSystem::COORDSYSTEM_WORLD;
        GetCoordSystem(robhandle,MecName.c_str(),&coord);
        TeachRobTarget(robhandle,MecName.c_str(),currtool.c_str(),currobj.c_str(),coord,curpos);


        for(int i=0;i<rob_DI.keys().size();++i)
        {
            QString name= rob_DI.keys().at(i);
            IoItem item;
            GetExtraIOData(robhandle,&item,"DI",name.toStdString().c_str());
            //            cout<<"name : "<<item.name<<endl;
            //            cout<<"iotype : "<<item.ioType<<endl;
            //            cout<<"bustype : "<<item.busType<<endl;
            //            cout<<"unitNo : "<<item.unitNo<<endl;
            //            cout<<"unitmap : "<<item.unitMap<<endl;
            //            cout<<"datalen : "<<item.dataLen<<endl;
            //            cout<<"defaultvalue : "<<item.defaultValue<<endl;
            //            cout<<"value : "<<item.value<<endl;
            //            cout<<"bActived : "<<item.bActived<<endl;
            //            cout<<"bsimulation : "<<item.bSimulation<<endl;
            //            cout<<"connect : "<<item.connect<<endl;
            rob_DI[name]=item.value;
        }
        for(int i=0;i<rob_DO.keys().size();++i)
        {
            QString name= rob_DO.keys().at(i);
            IoItem item;
            GetExtraIOData(robhandle,&item,"DO",name.toStdString().c_str());
            rob_DO[name] = item.value;
//            cout<<"name : "<<item.name<<endl;
//            cout<<"iotype : "<<item.ioType<<endl;
//            cout<<"bustype : "<<item.busType<<endl;
//            cout<<"unitNo : "<<item.unitNo<<endl;
//            cout<<"unitmap : "<<item.unitMap<<endl;
//            cout<<"datalen : "<<item.dataLen<<endl;
//            cout<<"defaultvalue : "<<item.defaultValue<<endl;
//            cout<<"value : "<<item.value<<endl;
//            cout<<"bActived : "<<item.bActived<<endl;
//            cout<<"bsimulation : "<<item.bSimulation<<endl;
//            cout<<"connect : "<<item.connect<<endl;
        }
        msleep(1);
    }

    DisconnectRobotController(robhandle);
}
